#include "AlgoCDP.h"
#include <iostream>
#include <vector>
#include <random>
#include <iomanip>
#include <tuple>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

struct Result 
{
    std::vector<double> sonars;
    double target;
};

Result runBellhopSimulations(int nums) 
{
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> lon_dist(80.0, 90.0);
    std::uniform_real_distribution<> lat_dist(0, 10.0);
    std::uniform_real_distribution<> sonar(0, 1);

    std::vector<double> ALon;
    std::vector<double> ALat;
    std::vector<double> all_results;

    for (int i = 0; i < nums; ++i) 
    {
        all_results.push_back(sonar(gen));
        ALon.push_back(lon_dist(gen));
        ALat.push_back(lat_dist(gen));
    }

    std::sort(ALon.begin(), ALon.end());
    std::sort(ALat.begin(), ALat.end());

    int nums_x = ALon[nums - 1] - ALon[0];
    int nums_y = ALat[nums - 1] - ALat[0];
    int grid_nums = nums_x * nums_y;
    double target_probability = 1.000 / grid_nums;

    return { all_results, target_probability };
}

int main()
{
    const int nums = 10;

    auto result = runBellhopSimulations(nums);
    std::vector<double> sonar_distribution = result.sonars;
    double target_probability = 0.01;

    algo::CDP calculator;
    std::vector<double> cdp_results = calculator.calculateCDP(target_probability, sonar_distribution);
    std::vector<double> cpcd_results = calculator.calculateCPCD(sonar_distribution);

    std::cout << std::endl;
    std::cout << "sonar:";
    for (double val : sonar_distribution) {
        std::cout << val << " ";
    }
    std::cout << std::endl;
    std::cout << "CDP:";
    for (double val : cdp_results) {
        std::cout << val << " ";
    }
    std::cout << std::endl;

    std::cout << "CPCD:";
    for (double val : cpcd_results) {
        std::cout << val << " ";
    }
    std::cout << std::endl;



    {
        std::vector<double> single_sonar;
        single_sonar.push_back(sonar_distribution.back());

        std::vector<double> cdp_results = calculator.calculateCDP(target_probability, single_sonar);
        std::vector<double> cpcd_results = calculator.calculateCPCD(single_sonar);

        std::cout << std::endl;
        std::cout << "sonar:";
        for (double val : sonar_distribution) {
            std::cout << val << " ";
        }
        std::cout << std::endl;
        std::cout << "CDP:";
        for (double val : cdp_results) {
            std::cout << val << " ";
        }
        std::cout << std::endl;

        std::cout << "CPCD:";
        for (double val : cpcd_results) {
            std::cout << val << " ";
        }
        std::cout << std::endl;
    }



    return 0;
}
